#pragma once
#include "spline.h"
#include "tool.hpp"

using namespace std;

/**
 * @brief pp样条
 *
 * @tparam Dim 维度,一维的曲线没有意义,作为函数的标记,大于一维则是曲线
 * @tparam Order 阶数
 * @param knots_curve Dim×N的矩阵，列向量为曲线的节点,如果是函数拟合则是2×N的矩阵,分别是自变量和因变量
 * @param bctp 边界条件类型
 * @param bc 边界条件,视类型决定有无输入
 */
template <int Dim, int Order>
void Spline<Dim, Order, PiecewiseP>::fitCurve(const vector<vector<double>> &knots_curve, BoundConditionType bctp, const vector<vector<double>> &bc)
{
    int N = knots_curve[0].size(); // 节点个数
    int isfun = 0;                 // 标记开始插值的维度索引
    // 1维作为函数的标记
    if (Dim == 1)
    {
        isfun = 1; // 标记
        knots = knots_curve[0];
    }
    else // 计算累计弦长
    {
        knots.resize(N);
        knots[0] = 0;
        for (int i = 1; i < N; ++i)
        {
            double s = 0;
            for (int j = 0; j < Dim; ++j)
            {
                s += pow(knots_curve[j][i] - knots_curve[j][i - 1], 2);
            }
            knots[i] = knots[i - 1] + sqrt(s);
        }
    }
    if (Order == 1)
    {
        // 直接计算一阶多项式
        polys.resize(Dim);
        for (int i = isfun; i < Dim + isfun; ++i)
        {
            polys[i - isfun].resize(N - 1);
            for (int j = 0; j < N - 1; ++j)
            {
                vector<double> coef{(knots_curve[i][j] * knots[j + 1] - knots_curve[i][j + 1] * knots[j]) / (knots[j + 1] - knots[j]),
                                    (knots_curve[i][j + 1] - knots_curve[i][j]) / (knots[j + 1] - knots[j])};
                polys[i - isfun][j] = Polynomial(coef);
            }
        }
    }
    else if (Order == 3)
    {
        // 计算系数矩阵
        vector<vector<double>> MAT(N, vector<double>(N, 0));
        if (bctp == Complete)
        {
            MAT[0][0] = 2;
            MAT[0][1] = 1;
            MAT[N - 1][N - 1] = 2;
            MAT[N - 1][N - 2] = 1;
        }
        else if (bctp == SpecifiedSecondDerivatives || bctp == Natural)
        {
            MAT[0][0] = 1;
            MAT[N - 1][N - 1] = 1;
        }
        else
        {
            throw;
        }
        for (int j = 2; j <= N - 1; ++j)
        {
            MAT[j - 1][j - 2] = (knots[j - 1] - knots[j - 2]) / (knots[j] - knots[j - 2]);
            MAT[j - 1][j] = (knots[j] - knots[j - 1]) / (knots[j] - knots[j - 2]);
            MAT[j - 1][j - 1] = 2.0;
        }

        vector<vector<double>> divided_differences_table(N, vector<double>(Order, 0.0)); // 差商表
        vector<double> RHS(N, 0);                                                        // 右端项

        polys.resize(Dim);
        // 分别对各个维度样条插值
        for (int i = isfun; i < Dim + isfun; ++i)
        {
            // 计算差商表
            for (int j = 0; j < N; ++j)
            {
                divided_differences_table[j][0] = knots_curve[i][j];
            }
            for (int k = 1; k < Order; ++k)
            {
                for (int j = k; j < N; ++j)
                {
                    divided_differences_table[j][k] =
                        (divided_differences_table[j][k - 1] - divided_differences_table[j - 1][k - 1]) /
                        (knots[j] - knots[j - k]);
                }
            }
            // 计算右端项
            if (bctp == Complete)
            {
                RHS[0] = 6 * ((knots_curve[i][1] - knots_curve[i][0]) / pow(knots[1] - knots[0], 2) - bc[i - isfun][0] / (knots[1] - knots[0]));
                RHS[N - 1] = 6 * ((knots_curve[i][N - 2] - knots_curve[i][N - 1]) / pow(knots[N - 2] - knots[N - 1], 2) - bc[i - isfun][1] / (knots[N - 2] - knots[N - 1]));
            }
            else if (bctp == SpecifiedSecondDerivatives)
            {
                RHS[0] = bc[i - isfun][0];
                RHS[N - 1] = bc[i - isfun][1];
            }
            else if (bctp == Natural)
            {
                RHS[0] = 0;
                RHS[N - 1] = 0;
            }
            for (int j = 1; j < N - 1; ++j)
            {
                RHS[j] = 6 * divided_differences_table[j + 1][Order - 1];
            }

            // 求解二阶导
            vector<double> M = solve(MAT, RHS);
            // 计算样条
            Polynomial p(vector<double>{-knots[0], 1.0});
            polys[i - isfun].resize(N - 1);
            if (bctp == Complete)
            {
                polys[i - isfun][0] = ((M[1] - M[0]) / 6 / (knots[1] - knots[0]) * (p ^ 3) + M[0] / 2 * (p ^ 2) +
                                       bc[i - isfun][0] * p + knots_curve[i][0]);
            }
            else if (bctp == SpecifiedSecondDerivatives || bctp == Natural)
            {
                polys[i - isfun][0] = (M[1] - M[0]) / 6 / (knots[1] - knots[0]) * (p ^ 3) + M[0] / 2 * (p ^ 2) +
                                      ((knots_curve[i][1] - knots_curve[i][0]) / (knots[1] - knots[0]) - (2 * M[0] + M[1]) * (knots[1] - knots[0]) / 6) * p +
                                      knots_curve[i][0];
            }
            for (int j = 1; j < N - 1; ++j)
            {
                Polynomial p(vector<double>{-knots[j], 1.0});
                polys[i - isfun][j] = ((M[j + 1] - M[j]) / 6 / (knots[j + 1] - knots[j]) * (p ^ 3) + M[j] / 2 * (p ^ 2) +
                                       polys[i - isfun][j - 1].diff().evaluate(knots[j]) * p + knots_curve[i][j]);
            }
        }
    }
    else
    {
        throw;
    }
}

/**
 * @brief 根据基函数递归定义，返回基函数B^n_i的第pieceid段的多项式,pieceid从1到N-1
 *
 * @tparam Dim 维度
 * @tparam Order 阶数
 * @param n B样条基函数的阶
 * @param i B样条基函数的索引，取2-Order到N
 * @param pieceid 对应B样条基函数限定的区间[x1,xN],pieceid取1到N-1
 * @return const Polynomial& 基函数B^n_i的第pieceid段的多项式
 */
template <int Dim, int Order>
Polynomial Spline<Dim, Order, B>::setBasic(int n, int i, int pieceid) const
{
    // 递归出口
    if (n == 0)
    {
        if (pieceid == i - 1)
            return Polynomial(vector<double>{1.0});
        else
            return Polynomial(vector<double>{0.0});
    }
    // 递归式
    else
    {
        Polynomial p1(vector<double>{-knots[i - 1 + Order - 1], 1}), p2(vector<double>{knots[i + n + Order - 1], -1});
        return p1 * setBasic(n - 1, i, pieceid) / (knots[i + n - 1 + Order - 1] - knots[i - 1 + Order - 1]) + p2 * setBasic(n - 1, i + 1, pieceid) / (knots[i + n + Order - 1] - knots[i + Order - 1]);
    }
}

/**
 * @brief B样条曲线拟合函数，实现了周期性条件，可以拟合任意维度和阶数
 *
 * @tparam Dim 维度
 * @tparam Order 阶数
 * @param knots_curve 曲线节点矩阵，Dim×N，列向量为曲线的节点向量
 * @param bctp 边界条件类型，暂时用不到，留做扩展
 * @param bc 边界条件，暂时用不到，留做扩展
 */
template <int Dim, int Order>
void Spline<Dim, Order, B>::fitCurve(const vector<vector<double>> &knots_curve, BoundConditionType bctp, const vector<vector<double>> &bc)
{
    int N = knots_curve[0].size();
    int isfun = 0; // 标记开始插值的维度索引
    knots.resize(N + 2 * Order);
    // 1维作为函数的标记
    if (Dim == 1)
    {
        isfun = 1; // 标记
        for (int i = Order; i < N + Order; ++i)
        {
            knots[i] = knots_curve[0][i - Order];
        }
    }
    else // 计算累计弦长
    {
        knots[Order] = 0;
        for (int i = Order + 1; i < N + Order; ++i)
        {
            double s = 0;
            for (int j = 0; j < Dim; ++j)
            {
                s += pow(knots_curve[j][i - Order] - knots_curve[j][i - Order - 1], 2);
            }
            knots[i] = knots[i - 1] + sqrt(s);
        }
    }
    double h = (knots[N + Order - 1] - knots[Order]) / N;
    // 隐式定义的2*Order个节点，以节点的平均间隔长度扩展
    for (int i = Order - 1; i >= 0; --i)
    {
        knots[i] = knots[i + 1] - h;
        knots[N + 2 * Order - 1 - i] = knots[N + 2 * Order - 2 - i] + h;
    }
    // N+Order-1个基函数
    bbss.resize(N + Order - 1);
    for (int i = 2 - Order; i <= N; ++i)
    {
        // 每个基函数有N-1段多项式，即在区间[x1,xN]上
        bbss[i + Order - 2].resize(N - 1);
        for (int j = 1; j <= N - 1; ++j)
        {
            bbss[i + Order - 2][j - 1] = setBasic(Order, i, j);
        }
    }
    if (bctp == Periodic)
    {
        // 计算系数矩阵
        vector<vector<double>> MAT(N + Order - 1, vector<double>(N + Order - 1, 0));
        // 周期边界条件前N-1个条件，样条在节点处与曲线相等
        for (int j = 0; j < N - 1; ++j)
        {
            for (int k = 0; k < N + Order - 1; ++k)
            {
                MAT[j][k] = bbss[k][j].evaluate(knots[j + Order]);
            }
        }
        // 周期边界条件后Order个条件，样条两端0到Order-1阶导相等
        vector<vector<Polynomial>> s(N + Order - 1, vector<Polynomial>(2, Polynomial()));
        for (int k = 0; k < N + Order - 1; ++k)
        {
            s[k][0] = bbss[k][0];
            s[k][1] = bbss[k][N - 2];
        }
        for (int j = 0; j < Order; ++j)
        {
            for (int k = 0; k < N + Order - 1; ++k)
            {
                MAT[j + N - 1][k] = s[k][1].evaluate(knots[N - 1 + Order]) - s[k][0].evaluate(knots[Order]);
                s[k][0] = s[k][0].diff();
                s[k][1] = s[k][1].diff();
            }
        }
        // 分别对各个维度样条插值
        polys.resize(Dim);
        for (int i = isfun; i < Dim + isfun; ++i)
        {
            // 计算右端项
            vector<double> RHS(N + Order - 1, 0.0);
            for (int j = 0; j < N - 1; ++j)
            {
                RHS[j] = knots_curve[i][j];
            }
            // 得到B样条系数，S=Σa(i)*B(i)
            vector<double> coef = solve(MAT, RHS);
            // 计算样条的多项式
            polys[i - isfun].resize(N - 1);
            for (int j = 0; j < N - 1; ++j)
            {
                polys[i - isfun][j] = Polynomial(vector<double>{0});
                for (int k = 0; k < N + Order - 1; ++k)
                {
                    polys[i - isfun][j] = polys[i - isfun][j] + coef[k] * bbss[k][j];
                }
            }
        }
    }
    else
    {
        throw;
    }
}

/**
 * @brief 根据基函数递归定义，返回基函数B^n_i的第pieceid段的多项式,pieceid从1到N-1
 *
 * @tparam Dim 维度
 * @tparam Order 阶数
 * @param n 基数B样条基函数的阶
 * @param i 基数B样条基函数的索引，取2-Order到N
 * @param pieceid 对应基数B样条基函数限定的区间[x1,xN],pieceid取1到N-1
 * @return const Polynomial& 基函数B^n_i的第pieceid段的多项式
 */
template <int Dim, int Order>
Polynomial Spline<Dim, Order, CardinalB>::setBasic(int n, int i, int pieceid) const
{
    // 递归出口
    if (n == 0)
    {
        if (pieceid == i - 1)
            return Polynomial(vector<double>{1.0});
        else
            return Polynomial(vector<double>{0.0});
    }
    // 递归式
    else
    {
        Polynomial p1(vector<double>{-knots[i - 1 + Order - 1], 1}), p2(vector<double>{knots[i + n + Order - 1], -1});
        return p1 * setBasic(n - 1, i, pieceid) / (knots[i + n - 1 + Order - 1] - knots[i - 1 + Order - 1]) + p2 * setBasic(n - 1, i + 1, pieceid) / (knots[i + n + Order - 1] - knots[i + Order - 1]);
    }
}

/**
 * @brief 基数B样条 按照定理3.57和定理3.58
 *
 * @tparam Dim 维度
 * @tparam Order 阶数
 * @param knots_curve 曲线节点
 * @param bctp 边界条件类型
 * @param bc 边界条件
 */
template <int Dim, int Order>
void Spline<Dim, Order, CardinalB>::fitCurve(const vector<vector<double>> &knots_curve, BoundConditionType bctp, const vector<vector<double>> &bc)
{
    int isfun = 0;
    // 因为基数B样条的节点必须是整数,所以暂时不支持对曲线插值,只能插值函数
    if (Dim == 1)
    {
        isfun = 1;
    }
    else
    {
        throw;
    }
    if (Order == 2)
    {
        int N = knots_curve[0].size() + 1;
        knots.resize(N + 2 * Order);
        // 定理3.58中特殊的边界条件
        if (bctp == End)
        {
            for (int i = Order; i < N - 1 + Order; ++i)
            {
                knots[i] = (knots_curve[0][i - Order] - 1.0 / 2);
            }
            knots[N - 1 + Order] = knots[N - 2 + Order] + 1;
            // 隐式定义的2*Order个节点，以节点的平均间隔长度扩展
            for (int i = Order - 1; i >= 0; --i)
            {
                knots[i] = knots[i + 1] - 1;
                knots[N + 2 * Order - 1 - i] = knots[N + 2 * Order - 2 - i] + 1;
            }
            // N+Order-1个基函数
            bbss.resize(N + Order - 1);
            for (int i = 2 - Order; i <= N; ++i)
            {
                // 每个基函数有N-1段多项式，即在区间[x1,xN]上
                bbss[i + Order - 2].resize(N - 1);
                for (int j = 1; j <= N - 1; ++j)
                {
                    bbss[i + Order - 2][j - 1] = setBasic(Order, i, j);
                }
            }
            // 分别对各个维度样条插值
            polys.resize(Dim);
            for (int i = isfun; i < Dim + isfun; ++i)
            {
                // 计算系数矩阵和右端项
                vector<vector<double>> MAT(N + Order - 1, vector<double>(N + Order - 1, 0));
                vector<double> RHS(N + Order - 1, 0.0);
                // 边界条件
                MAT[0][0] = 1;
                MAT[0][1] = 1;
                RHS[0] = 2 * bc[i - isfun][0];
                MAT[N + Order - 2][N + Order - 2] = 1;
                MAT[N + Order - 2][N + Order - 3] = 1;
                RHS[N + Order - 2] = 2 * bc[i - isfun][1];
                // 样条在节点处与曲线相等
                MAT[1][1] = 5;
                MAT[1][2] = 1;
                MAT[N + Order - 3][N + Order - 3] = 5;
                MAT[N + Order - 3][N + Order - 4] = 1;
                RHS[1] = 8 * knots_curve[i][0] - 2 * bc[i - isfun][0];
                RHS[N + Order - 3] = 8 * knots_curve[i][N - 1] - 2 * bc[i - isfun][1];
                for (int j = 2; j < N; ++j)
                {
                    MAT[j][j] = 6;
                    MAT[j][j - 1] = 1;
                    MAT[j][j + 1] = 1;
                    RHS[j] = 8 * knots_curve[i][j - 1];
                }
                // 得到B样条系数，S=Σa(i)*B(i)
                vector<double> coef = solve(MAT, RHS);
                // 计算样条的多项式
                polys[i - isfun].resize(N - 1);
                for (int j = 0; j < N - 1; ++j)
                {
                    polys[i - isfun][j] = Polynomial(vector<double>{0});
                    for (int k = 0; k < N + Order - 1; ++k)
                    {
                        polys[i - isfun][j] = polys[i - isfun][j] + coef[k] * bbss[k][j];
                    }
                }
            }
        }
        else
        {
            throw;
        }
    }
    else if (Order == 3)
    {
        int N = knots_curve[0].size();
        knots.resize(N + 2 * Order);
        if (bctp == Complete)
        {
            for (int i = Order; i < N + Order; ++i)
            {
                knots[i] = knots_curve[0][i - Order];
            }
            // 隐式定义的2*Order个节点，以节点的平均间隔长度扩展
            for (int i = Order - 1; i >= 0; --i)
            {
                knots[i] = knots[i + 1] - 1;
                knots[N + 2 * Order - 1 - i] = knots[N + 2 * Order - 2 - i] + 1;
            }
            // N+Order-1个基函数
            bbss.resize(N + Order - 1);
            for (int i = 2 - Order; i <= N; ++i)
            {
                // 每个基函数有N-1段多项式，即在区间[x1,xN]上
                bbss[i + Order - 2].resize(N - 1);
                for (int j = 1; j <= N - 1; ++j)
                {
                    bbss[i + Order - 2][j - 1] = setBasic(Order, i, j);
                }
            }
            // 分别对各个维度样条插值
            polys.resize(Dim);
            for (int i = isfun; i < Dim + isfun; ++i)
            {
                // 计算系数矩阵和右端项
                vector<vector<double>> MAT(N + Order - 1, vector<double>(N + Order - 1, 0));
                vector<double> RHS(N + Order - 1, 0.0);
                // 边界条件
                MAT[0][0] = 1;
                MAT[0][2] = -1;
                RHS[0] = -2 * bc[i - isfun][0];
                MAT[N + Order - 2][N + Order - 2] = 1;
                MAT[N + Order - 2][N + Order - 4] = -1;
                RHS[N + Order - 2] = 2 * bc[i - isfun][1];
                // 样条在节点处与曲线相等
                MAT[1][1] = 2;
                MAT[1][2] = 1;
                MAT[N + Order - 3][N + Order - 3] = 2;
                MAT[N + Order - 3][N + Order - 4] = 1;
                RHS[1] = 3 * knots_curve[i][0] + bc[i - isfun][0];
                RHS[N + Order - 3] = 3 * knots_curve[i][N - 1] - bc[i - isfun][1];
                for (int j = 2; j < N; ++j)
                {
                    MAT[j][j] = 4;
                    MAT[j][j - 1] = 1;
                    MAT[j][j + 1] = 1;
                    RHS[j] = 6 * knots_curve[i][j - 1];
                }
                // 得到B样条系数，S=Σa(i)*B(i)
                vector<double> coef = solve(MAT, RHS);
                // 计算样条的多项式
                polys[i - isfun].resize(N - 1);
                for (int j = 0; j < N - 1; ++j)
                {
                    polys[i - isfun][j] = Polynomial(vector<double>{0});
                    for (int k = 0; k < N + Order - 1; ++k)
                    {
                        polys[i - isfun][j] = polys[i - isfun][j] + coef[k] * bbss[k][j];
                    }
                }
            }
        }
        else
        {
            throw;
        }
    }
}
